#pragma once

#include "pqueue.h"
#include <iostream>
using namespace std;

template <typename K, typename D, typename DC, typename KC>
pqueue<K,D,DC,KC>& pqueue<K,D,DC,KC>::push(const K &k, const D &d) 
{
  if (contains(k)) return replace(k, d, false);    // key exists --> replace
  heap_.push_back(pair_type(k, d));                // place new element at the back of heap
  upHeap(size()-1);                                // then push it up the heap
  assert(idxs_.size() == heap_.size());
  return *this;
}

template <typename K, typename D, typename DC, typename KC>
pqueue<K,D,DC,KC>& pqueue<K,D,DC,KC>::replace(const K &k, const D &d, bool check) 
{
  if (check && !contains(k)) return *this;         // key not exists --> exit
  int pos = idxs_[k], parent = (pos-1) / 2;
  heap_[pos] = pair_type(k, d);                    // replace with new data
  if (pos > 0 && less(heap_[pos], heap_[parent]))  // if parent is bigger
    upHeap(pos);                                   // then push it up
  else if (pos < size())                           // otherwise
    downHeap(pos);                                 // push it down the heap
  assert(idxs_.size() == heap_.size());
  return *this;
}

template <typename K, typename D, typename DC, typename KC>
pqueue<K,D,DC,KC>& pqueue<K,D,DC,KC>::remove(const K &k) 
{
  if (!contains(k)) return *this;
  int pos = idxs_[k], parent = (pos-1) / 2;        // get the position
  heap_[pos] = heap_.back();                       // move the last element here
  idxs_.erase(k);                                  // remove key from the key-map
  heap_.resize(size()-1);                          // remove the last element
  if (pos >= size()) return *this;
  if (pos > 0 && less(heap_[pos], heap_[parent]))  // if parent is bigger
    upHeap(pos);                                   // push it up
  else                                             // otherwise
    downHeap(pos);                                 // push it down
  assert(idxs_.size() == heap_.size());
  return *this;
}

template <typename K, typename D, typename DC, typename KC>
void pqueue<K,D,DC,KC>::upHeap(int pos)
{
  assert(pos >= 0 && pos < size());
  pair_type x = heap_[pos];                        // save element
  while (pos > 0) {
    int parent = (pos-1) / 2;
    if (!less(x, heap_[parent])) break;            // if x is at least parent, then exit
    setPos(pos, heap_[parent]);                    // otherwise, move parent down
    pos = parent;                                  // change position to parent
  }
  setPos(pos, x);                                  // place saved element here
}

template <typename K, typename D, typename DC, typename KC>
void pqueue<K,D,DC,KC>::downHeap(int pos)
{
  assert(pos >= 0 && pos < size());
  pair_type x = heap_[pos];                        // save element
  while (1) {
    int child = pos*2+1, child_right = pos*2+2;    // find smaller child
    if (child >= size()) break;
    if (child_right < size() && less(heap_[child_right], heap_[child]))
      child = child_right;
    if (!less(heap_[child], x)) break;             // if child is bigger then exit
    setPos(pos, heap_[child]);                     // otherwise, move child up
    pos = child;                                   // change position to child
  }
  setPos(pos, x);                                  // place saved element here
}

//template <typename K, typename D, typename DC, typename KC>
//void pqueue<K,D,DC,KC>::print() const
//{
//  cout << "heap (";
//  for (int i = 0; i < size(); i++)
//    cout << "{" << std::hex << heap_[i].first << ", " << std::dec << heap_[i].second << "}";
//  cout << ")\n";
//}
